#include"linearEqnSolver.h"

std::vector<double> solu(const std::vector<double> &_l,
                        const std::vector<double> &_u,
                        const std::vector<double> &_d,
                        const std::vector<double> &_b)
{
    int N = _d.size();
    Eigen::VectorXd d(N);
    Eigen::VectorXd b(N);
    for(int i = 0; i < N; i++){
        d[i] = _d[i];
        b[i] = _b[i];
    }
    Eigen::VectorXd l(N-1);
    Eigen::VectorXd u(N-1);
    for(int i = 0; i < N-1; i++){
        l[i] = _l[i];
        u[i] = _u[i];
    }

//    std::cout << "b:\n" << b << std::endl;

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(N, N); // 创建一个全零矩阵  
    A.diagonal() = d; // 设置主对角线向量  
    A.diagonal(1) = u; // 设置上次对角线向量  
    A.diagonal(-1) = l; // 设置下次对角线向量 

//    std::cout << "A:\n"  << A  << std::endl;
  
    Eigen::VectorXd x = A.fullPivLu().solve(b); // 求解线性方程组  
  
    // 打印解向量x的值  
//    std::cout << "Solution x:\n" << x << std::endl;

    std::vector<double> _x;
    for(int i = 0; i < N; i++){
        _x.push_back(x[i]);
    }
    return _x;
}

std::vector<double> solu(const std::vector<std::vector<double>> &_A, 
                        const std::vector<double> &_b)
{
    int N = _b.size();
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(N, N); // 创建一个全零矩阵  
    Eigen::VectorXd b(N);
    for(int i = 0; i < N; i++){
        b[i] = _b[i];
        for(int j = 0; j < N; j++){
            A(i, j) = _A[i][j];
        }
    }
    
//    std::cout << "A:\n"  << A  << std::endl;

    Eigen::VectorXd x = A.fullPivLu().solve(b); // 求解线性方程组  
  
    // 打印解向量x的值  
//    std::cout << "Solution x:\n" << x << std::endl;

    std::vector<double> _x;
    for(int i = 0; i < N; i++){
        _x.push_back(x[i]);
    }
    return _x;
}

void print_vector(const std::vector<double> &x){
    int N = x.size();
    for(int i = 0; i < N; i++){
        std::cout << x[i] << " ";
    }
    std::cout << std::endl;
}

void print_matrix(const std::vector<std::vector<double>> &A){
    int row = A.size();
    int col = A[0].size();
    for(int i = 0; i < row; i++){
        for(int j = 0; j < col; j++){
            std::cout << A[i][j] << " ";
        }
        std::cout << std::endl;
    }
}